#pragma once
#include <iostream>
#include <unistd.h>
#include <string.h>
#include "DroneStateMsg.h"
#include "ControlMsg.h"
#include "server.h"
#include "ThreadPool.h"
#include "ThreadPool.cpp"
#include <json/json.h>
#include <mutex>

using namespace std;
static int swarmNumber;
static std::vector<ControlMsg> droneControlVec;
static std::vector<DroneStateMsg> droneStateVec;
static std::string newGmmMapMsg;
mutex mutexLock;
mutex mutexLockState;

std::string swarmControlAndState2Json(std::vector<DroneStateMsg>& droneStateVec, std::vector<ControlMsg>& droneControlVec)
{
	Json::Value sendData;
	Json::Value states;
	int count = 0;
	for (int i = 0; i < droneStateVec.size(); ++i) {
		if (droneStateVec[i].droneNumber == -1) {
			continue;
		}
		else {
			count++;
			Json::Value data;
			data["droneNumber"] = droneStateVec[i].droneNumber;
			data["positionX"] = droneStateVec[i].position[0];
			data["positionY"] = droneStateVec[i].position[1];
			data["positionZ"] = droneStateVec[i].position[2];
			data["velocityX"] = droneStateVec[i].velocity[0];
			data["velocityY"] = droneStateVec[i].velocity[1];
			data["velocityZ"] = droneStateVec[i].velocity[2];
			data["mode"] = droneStateVec[i].mode;
			data["bridgeMessage"] = droneStateVec[i].bridgeMessage;
			states.append(data); 
		}
	}
	sendData["swarmSize"] = Json::Value(count);
	sendData["drones"] = Json::Value(states);

	Json::Value controls;
	mutexLock.lock();
	for (int i = 0; i < droneControlVec.size(); ++i) {
		if (droneControlVec[i].droneNumber == -1) {
			continue;
		}
		else {
			Json::Value controlData;
			controlData["droneNumber"] = droneControlVec[i].droneNumber;
			controlData["positionX"] = droneControlVec[i].position[0];
			controlData["positionY"] = droneControlVec[i].position[1];
			controlData["positionZ"] = droneControlVec[i].position[2];
			controlData["mode"] = droneControlVec[i].mode;
			controls.append(controlData); 
		}
	}
	mutexLock.unlock();
	sendData["control"] = Json::Value(controls);


	Json::FastWriter fw;
	return fw.write(sendData).c_str();
}


std::string droneControlAndState2Json(std::vector<DroneStateMsg>& droneStateVec, std::vector<ControlMsg>& droneControlVec, int droneNumber)
{
	mutexLockState.lock();
	Json::Value sendData;
	Json::Value states;
	int count = 0;
	for (int i = 0; i < droneStateVec.size(); ++i) {
		if (droneStateVec[i].droneNumber == -1 || droneStateVec[i].mode == "") {
			continue;
		}
		else {
			count++;
			Json::Value data;
			data["droneNumber"] = droneStateVec[i].droneNumber;
			data["positionX"] = droneStateVec[i].position[0];
			data["positionY"] = droneStateVec[i].position[1];
			data["positionZ"] = droneStateVec[i].position[2];
			data["velocityX"] = droneStateVec[i].velocity[0];
			data["velocityY"] = droneStateVec[i].velocity[1];
			data["velocityZ"] = droneStateVec[i].velocity[2];
			data["mode"] = droneStateVec[i].mode;
			data["bridgeMessage"] = droneStateVec[i].bridgeMessage;
			states.append(data); 
		}
	}
	sendData["swarmSize"] = Json::Value(count);
	sendData["drones"] = Json::Value(states);
	mutexLockState.unlock();
	Json::Value control;
	mutexLock.lock();
	for (int i = 0; i < droneControlVec.size(); ++i) {
		if (droneControlVec[i].droneNumber == -1) {
			continue;
		}
		else if(droneControlVec[i].droneNumber != droneNumber){
			continue;
		}
		else {
			control["droneNumber"] = droneControlVec[i].droneNumber;
			control["positionX"] = droneControlVec[i].position[0];
			control["positionY"] = droneControlVec[i].position[1];
			control["positionZ"] = droneControlVec[i].position[2];
			control["mode"] = droneControlVec[i].mode;
		}
	}
	mutexLock.unlock();
	sendData["control"] = Json::Value(control);


	Json::FastWriter fw;
	return fw.write(sendData).c_str();
}



std::string droneState2Json(std::vector<DroneStateMsg>& droneStateVec)
{
	mutexLockState.lock();
	Json::Value sendData;
	Json::Value vec;
	int count = 0;
	for (int i = 0; i < droneStateVec.size(); ++i) {
		if (droneStateVec[i].droneNumber == -1) {
			continue;
		}
		else {
			count++;
			Json::Value data;
			data["droneNumber"] = droneStateVec[i].droneNumber;
			data["positionX"] = droneStateVec[i].position[0];
			data["positionY"] = droneStateVec[i].position[1];
			data["positionZ"] = droneStateVec[i].position[2];
			data["velocityX"] = droneStateVec[i].velocity[0];
			data["velocityY"] = droneStateVec[i].velocity[1];
			data["velocityZ"] = droneStateVec[i].velocity[2];
			data["mode"] = droneStateVec[i].mode;
			data["bridgeMessage"] = droneStateVec[i].bridgeMessage;
			vec.append(data); 
		}
	}
	sendData["swarmSize"] = Json::Value(count);
	sendData["drones"] = Json::Value(vec);
	Json::FastWriter fw;
	mutexLockState.unlock();
	return fw.write(sendData).c_str();
}

int main(int argc,char** argv)
{
	std::cout << "If you want to know how to use, please use the -h command to see how to use it" << std::endl;
	if (argc >=2 && strcmp(argv[1],"-h")==0) {
		std::cout << "Welcome to swarm-bridge server" << std::endl;
		return 0;
	}
	std::cout<<"请输入此集群支持的最大无人机数量(大于0): ";
	std::cin>>swarmNumber;


	std::cout << "The maximum swarmSize is: " << swarmNumber << std::endl;
	droneStateVec.resize(swarmNumber);
	droneControlVec.resize(swarmNumber);
	int fd = socket(AF_INET, SOCK_STREAM, 0);
	if (fd == -1)
	{
		perror("socket");
		exit(0);
	}

	struct sockaddr_in addr;
	addr.sin_family = AF_INET;
	addr.sin_port = htons(8999);       
	addr.sin_addr.s_addr = INADDR_ANY; 
	int ret = bind(fd, (struct sockaddr*)&addr, sizeof(addr));
	if (ret == -1)
	{
		perror("bind");
		exit(0);
	}
	ret = listen(fd, 100);
	if (ret == -1)
	{
		perror("listen");
		exit(0);
	}


	int computeFd = socket(AF_INET, SOCK_STREAM, 0);
	if (computeFd == -1)
	{
		perror("socket");
		exit(0);
	}
	struct sockaddr_in computeAddr;
	computeAddr.sin_family = AF_INET;
	computeAddr.sin_port = htons(30002);      
	computeAddr.sin_addr.s_addr = INADDR_ANY; 
	int computeRet = bind(computeFd, (struct sockaddr*)&computeAddr, sizeof(computeAddr));
	if (computeRet == -1)
	{
		perror("bind");
		exit(0);
	}
	computeRet = listen(computeFd, 100);
	if (computeRet == -1)
	{
		perror("listen");
		exit(0);
	}

	ThreadPool<MyInfo> pool(3,10);
	PoolInfo poolInfo(fd, computeFd, &pool);
	MyInfo myInfo(&poolInfo);
	std::cout << "main pthread exits..." << std::endl;
	pool.addTask(Task<MyInfo>(acceptConn, &myInfo));
	pool.addTask(Task<MyInfo>(acceptComputeConn, &myInfo));
	sleep(50);

	while (pool.getBusyNum() > 1) {
		//sleep(30);
	}

	while (true);
	std::cout << "delete pool in main" << std::endl;
	std::cout << pool.getBusyNum() << std::endl;
	//delete &pool;
	pool.~ThreadPool();

	return 0;
}



void acceptComputeConn(void* arg){
	MyInfo myInfo = *(MyInfo*)arg;
	PoolInfo poolinfo = *(myInfo.poolInfo);
	socklen_t len = sizeof(struct sockaddr);
	while (1)
	{
		SockInfo* socketInfo = new SockInfo();
		socketInfo->fd = accept(poolinfo.computeFd, (struct sockaddr*)&socketInfo->addr, &len);
		if (socketInfo->fd == -1)
		{
			perror("accept");
			break;
		}
		myInfo.socketInfo = socketInfo;
		poolinfo.p->addTask(Task<MyInfo>(computeConn, &myInfo));

	}
	close(poolinfo.computeFd); 
	return;
}


void getControlResult(std::vector<ControlMsg>& droneControlVec, const std::string& recvMsg) {
	Json::Reader reader;
	Json::Value data;
	bool flag = data.size() == 5 && data.isMember("positionX");
	if (reader.parse(recvMsg.c_str(), data))
	{
		if(flag){	
			int index = data["droneNumber"].asInt();
			droneControlVec[index].droneNumber = index;
			droneControlVec[index].position[0] = data["positionX"].asDouble();
			droneControlVec[index].position[1] = data["positionY"].asDouble();
			droneControlVec[index].position[2] = data["positionZ"].asDouble();
			droneControlVec[index].mode = data["mode"].asString();
		}
		else{
			for(int i = 0; i < data.size(); ++i){
				int index = data[i]["droneNumber"].asInt();
				if(index == -1){
					continue;
				}
				droneControlVec[index].droneNumber = index;
				droneControlVec[index].position[0] = data[i]["positionX"].asDouble();
				droneControlVec[index].position[1] = data[i]["positionY"].asDouble();
				droneControlVec[index].position[2] = data[i]["positionZ"].asDouble();
				droneControlVec[index].mode = data[i]["mode"].asString();
			}
		}
	}
	else {
		std::cout << "control vector convert fails..." << std::endl;
	}
}

void computeConn(void* arg){
	MyInfo myInfo = *(MyInfo*)arg;
	char ip[32];
	printf("control & compute client's IP:%s, port:%d\n", inet_ntop(AF_INET, &myInfo.socketInfo->addr.sin_addr.s_addr, ip, sizeof(ip)),ntohs(myInfo.socketInfo->addr.sin_port));

	while (1)
	{
		char buf[sizeof(int)];
		int ret = read(myInfo.socketInfo->fd, buf, sizeof(int));
		if (ret == sizeof(int))
		{	

			int recvLength;
			memcpy(&recvLength, buf, sizeof(int));
			char rbuf[recvLength];
			char* cur = rbuf;
			int rlen = read(myInfo.socketInfo->fd, cur, recvLength);

			while (rlen < recvLength) {
				std::cout << "control in loop ... "<< recvLength << std::endl;
				recvLength -= rlen;
				cur += rlen;
				rlen = read(myInfo.socketInfo->fd, cur, recvLength);
			}

			// std::cout<<rbuf<<std::endl;
			if(strncmp(rbuf,"REFRESH",recvLength) == 0){
				std::cout<<" 更新数据 "<< std::endl;
				//重发一遍当前状态
				std::string sendString = droneState2Json(droneStateVec);
				sendString.append(1, '\0');
				int sendActualLength = sendString.size();
				// std::cout<<sendString;
				int sendLength = sendActualLength + sizeof(int);
				char sbuf[sendLength];
				memcpy(sbuf, &sendActualLength, sizeof(int));
				memmove(sbuf + sizeof(int), sendString.c_str(), sendString.size());
				int byteNum = write(myInfo.socketInfo->fd, sbuf, sendLength);
				if (byteNum != sendLength) {
					std::cout << "Error when write to send buffer!" << std::endl;
				}
				continue;
			}
			else if(strncmp(rbuf,"ALLOFFBOARD",recvLength) == 0){
				std::cout<<" 全体任务飞行 "<< std::endl;
				mutexLock.lock();
				droneControlVec.clear();
				droneControlVec.resize(swarmNumber);
				for(int i = 0; i < swarmNumber; ++i){
					droneControlVec[i].droneNumber = i;
					droneControlVec[i].mode = "OFFBOARD";
				}
				mutexLock.unlock();
				continue;
			}
			else if(strncmp(rbuf,"ALLLANDING",recvLength) == 0){
				std::cout<<" 全体降落 "<< std::endl;
				mutexLock.lock();
				droneControlVec.clear();
				droneControlVec.resize(swarmNumber);
				for(int i = 0; i < swarmNumber; ++i){
					droneControlVec[i].droneNumber = i;
					droneControlVec[i].mode = "LANDING";
				}
				mutexLock.unlock();
				continue;
			}
			else if(strncmp(rbuf,"ALLMANUAL",recvLength) == 0){
				std::cout<<" 全体手控 "<< std::endl;
				mutexLock.lock();
				droneControlVec.clear();
				droneControlVec.resize(swarmNumber);
				for(int i = 0; i < swarmNumber; ++i){
					droneControlVec[i].droneNumber = i;
					droneControlVec[i].mode = "MANUAL";
				}
				mutexLock.unlock();
				continue;
			}
			else if(strncmp(rbuf,"ALLDISCONNECTED",recvLength) == 0){
				std::cout<<" 全体取消控制 "<< std::endl;
				continue;
			}
			else if(strncmp(rbuf,"SWARMSIZE",recvLength) == 0){
				Json::Value sendData;
				sendData["allSwarmSize"] = swarmNumber;
				Json::FastWriter fw;
				std::string sendString = fw.write(sendData).c_str();;
				sendString.append(1, '\0');
				int sendActualLength = sendString.size();
				// std::cout<<sendString;
				int sendLength = sendActualLength + sizeof(int);
				char sbuf[sendLength];
				memcpy(sbuf, &sendActualLength, sizeof(int));
				memmove(sbuf + sizeof(int), sendString.c_str(), sendString.size());
				int byteNum = write(myInfo.socketInfo->fd, sbuf, sendLength);
				if (byteNum != sendLength) {
					std::cout << "Error when write to send buffer!" << std::endl;
				}
				continue;
			}
			// std::cout<<" 单个控制 "<< std::endl;
			mutexLock.lock();
			droneControlVec.clear();
			droneControlVec.resize(swarmNumber);
			getControlResult(droneControlVec, rbuf);
			mutexLock.unlock();
			std::string sendString = droneState2Json(droneStateVec);
			sendString.append(1, '\0');
			int sendActualLength = sendString.size();
			// std::cout<<sendString;
			int sendLength = sendActualLength + sizeof(int);
			char sbuf[sendLength];
			memcpy(sbuf, &sendActualLength, sizeof(int));
			memmove(sbuf + sizeof(int), sendString.c_str(), sendString.size());
			int byteNum = write(myInfo.socketInfo->fd, sbuf, sendLength);
			if (byteNum != sendLength) {
				std::cout << "Error when write to send buffer!" << std::endl;
			}
		}
		else if (ret == 0)
		{
			std::cout << "socket shutdown..." << std::endl;
			myInfo.socketInfo->fd = -1;
			break;
		}
		else
		{
			std::cout << "receive fails......" << std::endl;
			myInfo.socketInfo->fd = -1;
			break;
		}
	}
	close(myInfo.socketInfo->fd);
	mutexLock.lock();
	droneControlVec.clear();
	droneControlVec.resize(swarmNumber);
	mutexLock.unlock();
	return;
}

void acceptConn(void* arg){
	MyInfo myInfo = *(MyInfo*)arg;
	PoolInfo poolinfo = *(myInfo.poolInfo);
	socklen_t len = sizeof(struct sockaddr);
	while (1)
	{
		SockInfo* socketInfo = new SockInfo();
		socketInfo->fd = accept(poolinfo.fd, (struct sockaddr*)&socketInfo->addr, &len);
		if (socketInfo->fd == -1)
		{
			perror("accept");
			break;
		}
		myInfo.socketInfo = socketInfo;
		poolinfo.p->addTask(Task<MyInfo>(working, &myInfo));

	}
	close(poolinfo.fd);
	return;
}


void working(void* arg)
{

	MyInfo myInfo = *(MyInfo*)arg;
	char ip[32];
	printf("drone communication client's IP:%s, port:%d\n", inet_ntop(AF_INET, &myInfo.socketInfo->addr.sin_addr.s_addr, ip, sizeof(ip)),ntohs(myInfo.socketInfo->addr.sin_port));
	int endDn = -1;
	while (1)
	{
		char buf[sizeof(int)];
		int ret = read(myInfo.socketInfo->fd, buf, sizeof(int));
		if (ret == sizeof(int))
		{	
			int recvLength;
			memcpy(&recvLength, buf, sizeof(int));
			char rbuf[recvLength];
			char* cur = rbuf;
			int rlen = read(myInfo.socketInfo->fd, cur, recvLength);
			while (rlen < recvLength) {
				std::cout << "in loop ... "<< rlen << std::endl;
				recvLength -= rlen;
				cur += rlen;
				rlen = read(myInfo.socketInfo->fd, cur, recvLength);
			}
			DroneStateMsg droneMsg;
			droneMsg.getDroneState(rbuf);
			// std::cout << "==============" << std::endl;
			// std::cout << droneMsg << std::endl;
			// 清空
			endDn = droneMsg.droneNumber;
			mutexLockState.lock();
			droneStateVec[droneMsg.droneNumber]= DroneStateMsg();
			// 更新
			droneStateVec[droneMsg.droneNumber]= droneMsg;
			mutexLockState.unlock();
			std::string sendString;
			// sendString = droneState2Json(droneStateVec);
			sendString = droneControlAndState2Json(droneStateVec, droneControlVec, endDn);
			sendString.append(1, '\0');
			std::cout << "sending to: " << endDn << std::endl;
			std::cout<<sendString<<std::endl;
			int sendActualLength = sendString.size();
			int sendLength = sendActualLength + sizeof(int);
			char sbuf[sendLength];
			memcpy(sbuf, &sendActualLength, sizeof(int));
			memmove(sbuf + sizeof(int), sendString.c_str(), sendString.size());
			int byteNum = write(myInfo.socketInfo->fd, sbuf, sendLength);	
			if (byteNum != sendLength) {
				std::cout << "Error when write to send buffer!" << std::endl;
			}
		}
		else if (ret == 0)
		{
			std::cout << "socket shutdown..." << std::endl;
			myInfo.socketInfo->fd = -1;
			break;
		}
		else
		{
			std::cout << "receive fails......" << std::endl;
			myInfo.socketInfo->fd = -1;
			break;
		}
	}
	close(myInfo.socketInfo->fd);
	mutexLockState.lock();
	droneStateVec[endDn]= DroneStateMsg();
	mutexLockState.unlock();
	return;
}
